using System;
using System.Collections.Generic;
using System.Text;
using PandoraCommon;

namespace ChronoGPS
{
    /// <summary>
    /// Contains the necessary functions to calculate acceleration values from GPS track positions.
    /// </summary>
    public class Gforce
    {

        /// <summary>
        /// Calculates the lateral acceleration for a given position (p2), in relation to the previous position (p1), in Gs.
        /// </summary>
        /// <remarks>Formula used with help from Soren Rasmussen</remarks>
        /// <param name="p1">The previous position.</param>
        /// <param name="p2">The current position.</param>
        /// <returns>Lateral acceleration, in Gs</returns>
        public static double CalculateLateralAcceleration(GpsPosition p1, GpsPosition p2)
        {
            double headingDiff =  p2.Heading - p1.Heading;

            if (headingDiff < -180) 
                headingDiff += 360;
            else if (headingDiff > 180) 
                headingDiff -= 360;

            // LatG(n)=1000*1000/(3600*9,80865)*Pi/180*Speed(n)*(Direction(n)-Direction(n-1))/(Time(n)-Time(n-1))
            double lateralAcceleration = 1000 * 1000 / (3600 * 9.80865) * Math.PI / 180 * p2.Speed * (headingDiff) / (p2.TimeSince(p1));

            return (-1) * lateralAcceleration;
        }


        /// <summary>
        /// Calculates the longitudinal acceleration for a given position (p2), in relation to the previous position (p1), in Gs.
        /// </summary>
        /// <remarks>Formula used with help from Soren Rasmussen</remarks>
        /// <param name="p1">The previous position.</param>
        /// <param name="p2">The current position.</param>
        /// <returns>Longitudinal acceleration, in Gs</returns>
        public static double CalculateLongitudinalAcceleration(GpsPosition p1, GpsPosition p2)
        {
            // LongG(n)=1000*1000/(3600*9,80865)*(Speed(n)-Speed(n-1))/(Time(n)-Time(n-1))
            //double longitudinalAcceleration = 1000 * 1000 / (3600 * 9.80865) * (p2.Speed - p1.Speed) / (p2.TimeSince(p1));
            double longitudinalAcceleration = 1000 * 1000 / (3600 * 9.80865) * (p2.Speed - p1.Speed) / (p2.TimeSince(p1));

            return longitudinalAcceleration * 10;
        }

    }
}
